#include "filtering.h"
#include "my_point_types.h"

//PCL
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/impl/normal_3d_omp.hpp>
#include <pcl/features/feature.h>
#include <pcl/features/impl/feature.hpp>
#include <pcl/surface/mls.h>
//#include <pcl/surface/mls_omp.h>
//#include <pcl/surface/impl/mls_omp.hpp>

template <typename PointInT, typename PointOutT>
int estimateSIFT(const typename pcl::PointCloud<PointInT>::Ptr in_cloud,
				 typename pcl::PointCloud<PointOutT>::Ptr out_cloud,
				 int nr_octaves = 0,
				 float min_scale = 0,
				 int nr_scales_per_octave = 0,
				 float min_contrast = 0)
{
	pcl::SIFTKeypoint< PointInT, PointOutT > keypoint_detector ;
	keypoint_detector.setInputCloud(in_cloud);

	if ( (nr_octaves!=0) && (min_scale!=0) && (nr_scales_per_octave!=0) )
	{
		keypoint_detector.setScales (min_scale, nr_octaves, nr_scales_per_octave);
	}

	if (min_contrast!=0)
	{
		keypoint_detector.setMinimumContrast(min_contrast);
	}
	//typename pcl::PointCloud<PointOutT>::Ptr out_ptr (new pcl::PointCloud<PointOutT>);


	keypoint_detector.compute(*out_cloud);
	return 1;
}


template <typename PointInT, typename PointOutT>
int compute_normals(const typename pcl::PointCloud<PointInT>::Ptr incloud,
					const float radius,
					const bool mode, //true if use knn, false if radius search
					typename pcl::PointCloud<PointOutT>::Ptr outcloud)
{
	typename pcl::NormalEstimationOMP<PointInT, PointOutT> normal_estimator;
	//typename pcl::PointCloud<PointOutT>::Ptr normals (new pcl::PointCloud<PointOutT>);

	if (mode) //use knn
	{
		int knn_radius = (int) radius; //cast to int
		normal_estimator.setKSearch(knn_radius);
	}

	else //use radius search
	{
		normal_estimator.setRadiusSearch(radius);
	}


	normal_estimator.setInputCloud (incloud);
	normal_estimator.setNumberOfThreads(4);
	normal_estimator.compute (*outcloud);


	return 1;
}


template <typename PointInT, typename PointOutT> //we consider in/out types will be the same
int smooth_mls(const typename pcl::PointCloud<PointInT>::Ptr incloud,
			   const MLSParameters params,
			   typename pcl::PointCloud<PointOutT>::Ptr outcloud)
{

	//create the smoothing object
	pcl::MovingLeastSquaresOMP< PointInT, PointOutT > smoother;
	smoother.setInputCloud(incloud);
	smoother.setSearchRadius(params.search_radius_);

	if( params.compute_normals_ == true) //do we want normals?
		smoother.setComputeNormals(true);

	if (params.polynomial_fit_ == true) //polynomial fit?
	{
		smoother.setPolynomialFit(true);
		smoother.setPolynomialOrder(params.order_);
		smoother.setSqrGaussParam(params.sqr_gauss_param_);
	}

	//DGM: DOES NOTHING ...YET?
	//switch (params.upsample_method_)
	//{
	//case (MLSParameters::NONE):
	//	{
	//		break;
	//	}

	//case (MLSParameters::SAMPLE_LOCAL_PLANE):
	//	{
	//		break;
	//	}


	//case (MLSParameters::RANDOM_UNIFORM_DENSITY):
	//	{
	//		break;
	//	}
	//}

	return 1;
}

int remove_outliers(sensor_msgs::PointCloud2::Ptr in_cloud, sensor_msgs::PointCloud2::Ptr out_cloud,int knn, float stddevtreshold)
{
	pcl::StatisticalOutlierRemoval<sensor_msgs::PointCloud2> sor;
	sor.setInputCloud(in_cloud);
	sor.setMeanK(knn);
	sor.setStddevMulThresh(stddevtreshold);

	sensor_msgs::PointCloud2::Ptr out_sm_cloud (new sensor_msgs::PointCloud2);
	sor.filter(*out_cloud);

	return 1;
}

//INSTANTIATING TEMPLATED FUNCTIONS
template int compute_normals<pcl::PointXYZ, pcl::PointNormal> (const  pcl::PointCloud<pcl::PointXYZ>::Ptr incloud,
															   const float radius,
															   const bool mode, //true if use knn, false if radius search
															   pcl::PointCloud<pcl::PointNormal>::Ptr outcloud);


template int estimateSIFT<pcl::PointXYZI, pcl::PointXYZ> (const pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud,
														  pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud,
														  int nr_octaves, float min_scale, int nr_scales_per_octave,
														  float min_contrast);

template int estimateSIFT<pcl::PointXYZRGB, pcl::PointXYZ> (const pcl::PointCloud<pcl::PointXYZRGB>::Ptr in_cloud,
															pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud,
															int nr_octaves, float min_scale, int nr_scales_per_octave,
															float min_contrast);
